#include <cmath>
#include <cstring>
#include "decoder_flac_frame.h"
#include "unicode.h"
#include "debug.h"

// ---------------------------------------------------
void FLACFrame::init(AudioReader* r, FLACstreaminfo *sinfo)
// ---------------------------------------------------
{
	frames[0].header.reader =
	frames[0].subframes[0].reader =
	frames[0].subframes[1].reader =
	frames[0].footer.reader =
	frames[1].header.reader =
	frames[1].subframes[0].reader =
	frames[1].subframes[1].reader =
	frames[1].footer.reader = r;

	frames[0].header.sinfo =
	frames[1].header.sinfo = sinfo;

	samplesLeft = 0;
	nextHeaderOk = false;
	nextSub1ok = false;
	nextSub0ok = false;
	nextOk = false;
	frames[0].subframes[0].isHeaderRead = false;
	frames[0].subframes[0].samplesRead = 0;
	frames[0].subframes[1].isHeaderRead = false;
	frames[0].subframes[1].samplesRead = 0;
	frames[1].subframes[0].isHeaderRead = false;
	frames[1].subframes[0].samplesRead = 0;
	frames[1].subframes[1].isHeaderRead = false;
	frames[1].subframes[1].samplesRead = 0;
}

//  flag - now
// !flag - next

// ---------------------------------------------------
bool FLACFrame::prepare()
// ---------------------------------------------------
{
	int substatus;
	// read frame header
	if (!frames[flag].header.read()) return false;
	// read first subframe
	
	if (frames[flag].header.channels == 9)
	{
		// side/right
		substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize+1, frames[flag].header.blockSize );
		if (substatus == 2) return false;
		if (substatus == 1)
		{
			substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize+1, frames[flag].header.blockSize );
			if (substatus == 2) return false;
		}

		substatus = frames[flag].subframes[1].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
		if (substatus == 2) return false;
		if (substatus == 1)
		{
			substatus = frames[flag].subframes[1].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
			if (substatus == 2) return false;
		}
	}
	else if (frames[flag].header.channels == 8 || frames[flag].header.channels == 10)
	{
		// left/side or mid/side
		substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
		if (substatus == 2) return false;
		if (substatus == 1)
		{
			substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
			if (substatus == 2) return false;
		}

		substatus = frames[flag].subframes[1].playerCall( frames[flag].header.sampleSize+1, frames[flag].header.blockSize );
		if (substatus == 2) return false;
		if (substatus == 1)
		{
			substatus = frames[flag].subframes[1].playerCall( frames[flag].header.sampleSize+1, frames[flag].header.blockSize );
			if (substatus == 2) return false;
		}
	}
	else if (frames[flag].header.channels == 0)
	{
		// mono
		substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
		if (substatus == 2) return false;
		if (substatus == 1)
		{
			substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
			if (substatus == 2) return false;
		}
	}
	else
	{
		// X channels; we assume 2
		substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
		if (substatus == 2) return false;
		if (substatus == 1)
		{
			substatus = frames[flag].subframes[0].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
			if (substatus == 2) return false;
		}

		substatus = frames[flag].subframes[1].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
		if (substatus == 2) return false;
		if (substatus == 1)
		{
			substatus = frames[flag].subframes[1].playerCall( frames[flag].header.sampleSize, frames[flag].header.blockSize );
			if (substatus == 2) return false;
		}
	}

	if (!frames[flag].footer.read()) return false;

	nextHeaderOk =
	nextSub0ok =
	nextSub1ok =
	nextOk = false;

	samplesLeft = frames[flag].header.blockSize;

	return true;
}

// ----------------------------------------------------------------------
bool FLACFrame::playerCall(int16_t *bufferL, int16_t *bufferR, uint32_t samples, uint32_t *count)
// ----------------------------------------------------------------------
{
	// fill the player's buffer with current frame's data
	uint16_t posD = frames[flag].header.blockSize - samplesLeft;
	uint32_t posP = 0;

	#ifdef DEBUG_FLAC_PROCESS
		iprintf("call: start\n");
		iprintf("call: %d to read from %d\n", samples, posD);
		scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
	#endif

	while (samples > 0 && samplesLeft > 0)
	{
		bufferL[posP] = frames[flag].subframes[0].samples[posD];
		bufferR[posP] = frames[flag].subframes[1].samples[posD];
		--samplesLeft;
		--samples;
		++posP;
		++posD;
		++(*count);
	}

	// read, or continue reading, the next frame
	
	if (!nextOk)
	{
		#ifdef DEBUG_FLAC_PROCESS
			iprintf("call: next frame is not ready\n");
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		#endif

		int substatus;

		if (!nextHeaderOk)
		{
			#ifdef DEBUG_FLAC_PROCESS
				iprintf("call: read frame header\n");
				scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
			// read frame header
			if (!frames[!flag].header.read()) return false;
			nextHeaderOk = true;
		}

		if (nextHeaderOk)
		{
			if (frames[!flag].header.channels == 9)
			{
				// side/right
				if (!nextSub0ok)
				{
					#ifdef DEBUG_FLAC_PROCESS
						iprintf("call: sub0 not ok yet\n");
						scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
					substatus = frames[!flag].subframes[0].playerCall( frames[!flag].header.sampleSize+1-frames[!flag].subframes[0].wasted, frames[!flag].header.blockSize );
					if (substatus == 0 && frames[!flag].header.blockSize <= 1024)
					{
						nextSub0ok = true;
					}
					else if (substatus == 0 && frames[!flag].header.blockSize > 1024)
					{
						nextSub0ok = true;
						return true;
					}
					else if (substatus == 1) return true;
					else if (substatus == 2) return false;
				}
				if (nextSub0ok && !nextSub1ok)
				{
					#ifdef DEBUG_FLAC_PROCESS
						iprintf("call: sub1 not ok yet\n");
						scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
					substatus = frames[!flag].subframes[1].playerCall( frames[!flag].header.sampleSize-frames[!flag].subframes[1].wasted, frames[!flag].header.blockSize );
					if (substatus == 0) nextSub1ok = true;
					else if (substatus == 1) return true;
					else if (substatus == 2) return false;
				}
			}
			else if (frames[!flag].header.channels == 8 || frames[!flag].header.channels == 10)
			{
				// left/side or mid/side
				if (!nextSub0ok)
				{
					#ifdef DEBUG_FLAC_PROCESS
						iprintf("call: sub0 not ok yet\n");
						scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
					substatus = frames[!flag].subframes[0].playerCall( frames[!flag].header.sampleSize-frames[!flag].subframes[0].wasted, frames[!flag].header.blockSize );
					if (substatus == 0 && frames[!flag].header.blockSize <= 1024)
					{
						nextSub0ok = true;
					}
					else if (substatus == 0 && frames[!flag].header.blockSize > 1024)
					{
						nextSub0ok = true;
						return true;
					}
					else if (substatus == 1) return true;
					else if (substatus == 2) return false;
				}
				if (nextSub0ok && !nextSub1ok)
				{
					#ifdef DEBUG_FLAC_PROCESS
						iprintf("call: sub1 not ok yet\n");
						scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
					substatus = frames[!flag].subframes[1].playerCall( frames[!flag].header.sampleSize+1-frames[!flag].subframes[1].wasted, frames[!flag].header.blockSize );
					if (substatus == 0) nextSub1ok = true;
					else if (substatus == 1) return true;
					else if (substatus == 2) return false;
				}
			}
			else if (frames[!flag].header.channels == 0)
			{
				// mono
				if (!nextSub0ok)
				{
					#ifdef DEBUG_FLAC_PROCESS
						iprintf("call: sub0 not ok yet\n");
						scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
					substatus = frames[!flag].subframes[0].playerCall( frames[!flag].header.sampleSize-frames[!flag].subframes[0].wasted, frames[!flag].header.blockSize );
					if (substatus == 0 && frames[!flag].header.blockSize <= 1024)
					{
						nextSub0ok = true;
					}
					else if (substatus == 0 && frames[!flag].header.blockSize > 1024)
					{
						nextSub0ok = true;
						return true;
					}
					else if (substatus == 1) return true;
					else if (substatus == 2) return false;
				}
				if (nextSub0ok) nextSub1ok = true;
			}
			else
			{
				// X channels; we assume 2
				if (!nextSub0ok)
				{
					#ifdef DEBUG_FLAC_PROCESS
						iprintf("call: sub0 not ok yet\n");
						scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
					substatus = frames[!flag].subframes[0].playerCall( frames[!flag].header.sampleSize-frames[!flag].subframes[0].wasted, frames[!flag].header.blockSize );
					if (substatus == 0 && frames[!flag].header.blockSize <= 1024)
					{
						nextSub0ok = true;
					}
					else if (substatus == 0 && frames[!flag].header.blockSize > 1024)
					{
						nextSub0ok = true;
						return true;
					}
					else if (substatus == 1) return true;
					else if (substatus == 2) return false;
				}
				if (nextSub0ok && !nextSub1ok)
				{
					#ifdef DEBUG_FLAC_PROCESS
						iprintf("call: sub1 not ok yet\n");
						scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
					substatus = frames[!flag].subframes[1].playerCall( frames[!flag].header.sampleSize-frames[!flag].subframes[1].wasted, frames[!flag].header.blockSize );
					if (substatus == 0) nextSub1ok = true;
					else if (substatus == 1) return true;
					else if (substatus == 2) return false;
				}
			}
		}

		if (nextHeaderOk && nextSub0ok && nextSub1ok)
		{
			if (!frames[!flag].footer.read()) return false;
			frames[!flag].jointRestore();
			nextOk = true;
			#ifdef DEBUG_FLAC_PROCESS
				iprintf("call: all ok\n");
				scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
		}
	}

	if (nextOk && samplesLeft == 0)
	{
		#ifdef DEBUG_FLAC_PROCESS
			iprintf("call: !SWITCH!\n");
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		#endif
		flag = !flag;
		nextHeaderOk =
		nextSub0ok =
		nextSub1ok =
		nextOk = false;
		samplesLeft = frames[flag].header.blockSize;
		posD = 0;
	}

	while (samples > 0 && samplesLeft > 0)
	{
		bufferL[posP] = frames[flag].subframes[0].samples[posD];
		bufferR[posP] = frames[flag].subframes[1].samples[posD];
		--samplesLeft;
		--samples;
		++posP;
		++posD;
		++(*count);
	}

	#ifdef DEBUG_FLAC_PROCESS
		iprintf("call(): end\n");
		scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
	#endif

	return true;
}

// ----------------------------------------------------------------------
void FLACFrame::Frame::jointRestore()
// ----------------------------------------------------------------------
{
	// restore joint stereo signal / duplicate mono
	uint32_t i = 0;
	int32_t tmp = 0;
	switch (header.channels)
	{
		case 10:
			// mid/side stereo
			for (; i<header.blockSize; ++i)
			{
				subframes[0].samples[i] <<= 1;
				tmp = (subframes[0].samples[i] | (subframes[1].samples[i] & 1));
				subframes[0].samples[i] = ((tmp + subframes[1].samples[i]) >> 1);
				subframes[1].samples[i] = ((tmp - subframes[1].samples[i]) >> 1);
			}
		break;

		case 8:
			// left/side stereo
			for (; i<header.blockSize; ++i)
			{
				subframes[1].samples[i] = subframes[0].samples[i] - subframes[1].samples[i];
			}
		break;

		case 9:
			// side/right stereo
			for (; i<header.blockSize; ++i)
			{
				subframes[0].samples[i] = subframes[0].samples[i] + subframes[1].samples[i];
			}
		break;
		
		case 0:
			// mono
			#ifdef ARM9
				dmaCopyWords(0, subframes[0].samples, subframes[1].samples, (header.blockSize<<2));
			#else
				memcpy(subframes[1].samples, subframes[0].samples, (header.blockSize<<2));
			#endif
		break;
	}

//	#ifdef DEBUG_FILEOUT
//		int16_t t;
//		for (int i=0; i<header.blockSize; ++i)
//		{
//			t = subframes[0].samples[i];
//			fwrite(&t, 2, 1, DBGFILE);
//			t = subframes[1].samples[i];
//			fwrite(&t, 2, 1, DBGFILE);
//		}
//	#endif
}

// ***************************************************
// ***************************************************
//
//                       HEADER
//
// ***************************************************
// ***************************************************

// ---------------------------------------------------
bool FLACFrame::Frame::Header::read()
// ---------------------------------------------------
{
	uint32_t tmp = 0;
	uint8_t x8[2] = {0, 0};
	while (!reader->eof())
	{
		// search for 1111 1111  1111 10 sync code
		if (!reader->read_byte_block(x8, 2, true)) return false;
		if (x8[0] != 0xFF && (x8[1] & 0xFC ) != 0xF8)
		{
			#ifdef DEBUG_DEC_FLAC_FRAME_HEADER
			iprintf("No sync code yet\n");
			#endif	
			continue;
		}
		
		// found sync code
		// read VBS flag
		isVBS = ((x8[1] & 0x1) == 1);
			
		// read block size
		if (!reader->read_uint32_be(&tmp, 4)) return false;
		bool bseoh = false; // block size end of header_

		switch (tmp)
		{
			case  0: return false; break; // 0000: reserved
			case  1: blockSize = 192; break; // 0001: 192
			case  2:
			case  3:
			case  4:
			case  5: blockSize = 576 << (tmp-2); break; // 0010-0101: 576*(2^(n-2))
			case  6: blockSize =  8; bseoh = true; break; // 0110: get  8b from end of header_
			case  7: blockSize = 16; bseoh = true; break; // 0111: get 16b from end of header_
			case  8:
			case  9:
			case 10:
			case 11:
			case 12:
			case 13:
			case 14:
			case 15: blockSize = 256 << (tmp-8); break; // 1000-1111: 256*(2^(n-8))

			default: return false;
		}

		// read sample rate
		if (!reader->read_uint32_be(&tmp, 4)) return false;
		bool sreoh = false; // sample rate end of header_

		switch (tmp)
		{
			case  0: sampleRate = sinfo->samplerate; break; // 0000: get from STREAMINFO metadata block
			case  1: sampleRate =  88200; break; // 0001:  88.2 kHz
			case  2: sampleRate = 176400; break; // 0010: 176.4 kHz
			case  3: sampleRate = 192000; break; // 0011: 192   kHz
			case  4: sampleRate =   8000; break; // 0100:   8   kHz
			case  5: sampleRate =  16000; break; // 0101:  16   kHz
			case  6: sampleRate =  22050; break; // 0110:  22.05kHz
			case  7: sampleRate =  24000; break; // 0111:  24   kHz
			case  8: sampleRate =  32000; break; // 1000:  32   kHz
			case  9: sampleRate =  44100; break; // 1001:  44.1 kHz
			case 10: sampleRate =  48000; break; // 1010:  48   kHz
			case 11: sampleRate =  96000; break; // 1011:  96   kHz
			case 12:
			case 13:
			case 14: sampleRate = tmp; sreoh = true; break; // 1100-1110 get n bits from header_ end

			default: return false; // 1111: invalid
		}
		
		// read channels
		if (!reader->read_uint32_be(&tmp, 4)) return false;
		if (tmp <= 10)
		{
			channels = tmp;
		}
		else // 1011-1111 reserved
		{
			return false;
		}
		
		// read sample size
		if (!reader->read_uint32_be(&tmp, 3)) return false;

		switch (tmp)
		{
			case 0: sampleSize = sinfo->bitspersample; break; // 000: get from STREAMINFO metadata block
			case 1: sampleSize =  8; break; // 001:  8 bps
			case 2: sampleSize = 12; break; // 010: 12 bps
			case 3: return false; break; // 011: reserved
			case 4: sampleSize = 16; break; // 100: 16 bps
			case 5: sampleSize = 20; break; // 101: 20 bps
			case 6: sampleSize = 24; break; // 110: 24 bps

			default: return false; // 111: reserved
		}

		if (!reader->skip_bits(1)) return false;

		// read sample/frame number
		if (!reader->read_utf8_uint64(&number, NULL, NULL)) return false;

		// end-of-block size
		if (bseoh)
		{
			if (!reader->read_uint32_be(&tmp, blockSize)) return false;
			blockSize = tmp + 1;
		}

		// end-of-sample rate
		if (sreoh)
		{
			if (sampleRate == 12) // 8b
			{
				reader->read_uint32_be(&tmp, 8);
				sampleRate = tmp;
			}
			else if (sampleRate == 13) // 16b
			{
				reader->read_uint32_be(&tmp, 16);
				sampleRate = tmp;
			}
			else if (sampleRate == 12) // 16b in tens
			{
				reader->read_uint32_be(&tmp, 16);
				sampleRate = tmp * 10;
			}
		}
		
		// CRC8
		if (!reader->read_byte_block(&crc8, 1, true)) return false;
		
		#ifdef DEBUG_DEC_FLAC_FRAME_HEADER
		iprintf("#%u VBS:%u Size: %u Hz:%u Ch:%u bps:%u crc:%x\n", (uint32_t)number, isVBS, blockSize, sampleRate, channels, sampleSize, crc8);
		#endif

		// sample size protect
		if (sampleSize != 8 && sampleSize != 16)
		{
			#ifdef DEBUG_DEC_FLAC_FRAME_HEADER
			iprintf("Unsupported sample size\n");
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
			return false;
		}

		return true;
	}

	#ifdef DEBUG_DEC_FLAC_FRAME_HEADER
	iprintf("FLAC H:EOF\n");
	#endif

	return false;
}

// ***************************************************
// ***************************************************
//
//                      SUBFRAME
//
// ***************************************************
// ***************************************************

// ---------------------------------------------------
bool FLACFrame::Frame::Subframe::readHeader()
// ---------------------------------------------------
{
	uint32_t tmpU = 0;
	wasted = 0;
	order = 0;

	if (!reader->read_uint32_be(&tmpU, 8)) return false;

	wasted = tmpU & 1;
	tmpU >>= 1;
	tmpU &= 0x3F;
	
	// read frame type
	if (tmpU == 0) // 000000
	{
		type = FLAC_SUBFRAME_CONSTANT;
	}
	else if (tmpU == 1) // 000001
	{
		type = FLAC_SUBFRAME_VERBATIM;
	}
	else if (8 <= tmpU && tmpU <= 15) // 001xxx (xxx = order)
	{
		type = FLAC_SUBFRAME_FIXED;
		order = (tmpU & 0x7);
		if (order > 4) // invalid
		{
			#ifdef DEBUG_DEC_FLAC_FRAME_SUB
			iprintf("FIXED order > 4\n");
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
			return false;
		}
	}
	else if (32 <= tmpU && tmpU <= 63) // 1xxxxx (xxxxx = order-1)
	{
		type = FLAC_SUBFRAME_LPC;
		order = (tmpU & 0x1F) + 1;
	}
	else // reserved or invalid
	{
		#ifdef DEBUG_DEC_FLAC_FRAME_SUB
		iprintf("LPC order invalid (%u)\n", tmpU);
		scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		#endif
		return false;
	}
	
	// read wasted bits
	if (wasted == 1)
	{
		reader->read_unary_unsigned(&tmpU);
		wasted = tmpU;
	}

	return true;
}

// ---------------------------------------------------
bool FLACFrame::Frame::Subframe::readConstant(const uint8_t _samplesize)
// ---------------------------------------------------
{
	int32_t tmpS = 0;
	if (!reader->read_int32_be(&tmpS, _samplesize)) return false;
//		if (!reader->read_int32_be(&tmpS, (header_.sampleSize - subf->wasted))) return false;
	samples[0] = tmpS;

	#ifdef DEBUG_DEC_FLAC_FRAME_SUB_CONSTANT
		iprintf("Sample = %d\n", samples[0]);
		scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
	#endif
	
	return true;
}

// ---------------------------------------------------
bool FLACFrame::Frame::Subframe::readVerbatim(const uint8_t _samplesize, const uint16_t _blocksize)
// ---------------------------------------------------
{
	int32_t tmpS = 0;

	// read samples
	for (uint32_t i=0; i<_blocksize; ++i)
	{
		if (!reader->read_int32_be(&tmpS, _samplesize)) return false;
		samples[i] = tmpS;
	}

	#ifdef DEBUG_DEC_FLAC_FRAME_SUB_VERBATIM
		for (uint32_t z=0; z<_blocksize; ++z)
		{
			iprintf("Sample[%02d] %d\n", z, samples[z]);
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		}
	#endif
	
	return true;
}

// ---------------------------------------------------
bool FLACFrame::Frame::Subframe::readFixed(const uint8_t _samplesize)
// ---------------------------------------------------
{
	int32_t  tmpS = 0;

	// read warm-up samples
	for (uint32_t i=0; i<order; ++i)
	{
		if (!reader->read_int32_be(&tmpS, _samplesize)) return false;
		samples[i] = tmpS;
	}

	#ifdef DEBUG_DEC_FLAC_FRAME_SUB_FIXED
		for (uint32_t z=0; z<order; ++z)
		{
			iprintf("Warm[%02d] %d\n", z, samples[z]);
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		}
	#endif

	return true;
}

// ---------------------------------------------------
bool FLACFrame::Frame::Subframe::readLpc(const uint8_t _samplesize)
// ---------------------------------------------------
{
	int32_t  tmpS = 0;
	uint32_t tmpU = 0;

	// read warm-up samples
	for (uint32_t i=0; i<order; ++i)
	{
		if (!reader->read_int32_be(&tmpS, _samplesize)) return false;
		samples[i] = tmpS;
	}	

	#ifdef DEBUG_DEC_FLAC_FRAME_SUB_LPC
		for (uint32_t z=0; z<order; ++z)
		{
			iprintf("Warm[%02d] %d\n", z, samples[z]);
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		}
	#endif

	// read LPC precision in bits
	if (!reader->read_uint32_be(&tmpU, 4)) return false;
	if (tmpU == 0xF) // 1111 invalid
	{
		#ifdef DEBUG_DEC_FLAC_FRAME_SUB_LPC
			iprintf("Precision invalid\n");
		#endif
		return false;
	}
	lpc.precision = tmpU + 1;

	// read LPC shift needed in bits
	if (!reader->read_uint32_be(&tmpU, 5)) return false;
	lpc.shift = (tmpU & 0xF) - (tmpU & 0x10);
	
	#ifdef DEBUG_DEC_FLAC_FRAME_SUB_LPC
		iprintf("Precision: %d\nShift: %d\n", lpc.precision, lpc.shift);		
		scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
	#endif

	// read LPC
	for (uint8_t i=0; i<order; ++i)
	{
		if (!reader->read_uint32_be(&tmpU, lpc.precision)) return false;
		lpc.vals[i] = ((tmpU & ((1<<(lpc.precision-1))-1)) - (tmpU & (1<<(lpc.precision-1))));
	}

	#ifdef DEBUG_DEC_FLAC_FRAME_SUB_LPC
		for (uint8_t z=0; z<order; ++z)
		{
			iprintf("lpc[%02d] %d\n", z, lpc.vals[z]);	
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		}
	#endif

	return true;
}

//
//	#ifdef DEBUG_TIMES_FLAC_SUBFRAME
//	u32 time = dbgTimerInt;
//	#endif
//	#ifdef DEBUG_TIMES_FLAC_SUBFRAME
//	iprintf("residuals - %u\n", dbgTimerInt-time);
//	#endif


// ------------------------------------------------
bool FLACFrame::Frame::Subframe::readResidualInit()
// ------------------------------------------------
{
	uint32_t tmpU;

	// read residual coding method and partition order
	if (!reader->read_uint32_be(&tmpU, 6)) return false;
	if ((tmpU & 0x20) == 1) // __1xxxxx: reserved
	{
		return false;
	}
	residual.isRice2 = ( (tmpU & 0x10) == 0x10 ); // __01xxxx - rice2 <- type
	residual.riceOrder = tmpU & 0x0F;
	residual.riceRead = 0;
	residual.ricePartitionsLeft = 1 << residual.riceOrder;

	#ifdef DEBUG_DEC_FLAC_FRAME_RESIDUAL
		(residual.isRice2) ? iprintf("RICE2 ") : iprintf("RICE ");
		iprintf("residual order:%d\n", residual.riceOrder);
		scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
	#endif

	return true;
}

// ------------------------------------------------
bool FLACFrame::Frame::Subframe::readResidualRice(const uint16_t _blocksize)
// ------------------------------------------------
{
	uint32_t tmpU = 0;
	int32_t  tmpS = 0;
	
	#ifdef DEBUGD_DEC_FLAC_FRAME_RESIDUAL_RICE
		iprintf("rice read start (%d)\n", toread);
		scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
	#endif
	
	while (residual.ricePartitionsLeft != 0)
	{
		#ifdef DEBUG_DEC_FLAC_FRAME_RESIDUAL_RICE
			iprintf("rice loop start (%d)\n", toread);
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		#endif

		// read header
		#ifdef DEBUG_DEC_FLAC_FRAME_RESIDUAL_RICE
			iprintf("rice header read\n");
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		#endif
		// read encoding parameter
		residual.partition.encoded = true;

		if (!residual.isRice2)
		{
			if (!reader->read_uint32_be(&tmpU, 4)) return false;
			if (tmpU == 0xF) // escape code (1111)
			{
				residual.partition.encoded = false;
			}
			else
			{
				residual.partition.parameter = tmpU;
			}
		}
		else
		{
			if (!reader->read_uint32_be(&tmpU, 5)) return false;
			if (tmpU == 0x1F) // escape code (11111)
			{
				residual.partition.encoded = false;
			}
			else
			{
				residual.partition.parameter = tmpU;
			}
		}

		if (!residual.partition.encoded)
		{
			if (!reader->read_uint32_be(&tmpU, 5)) return false;
			residual.partition.parameter = tmpU;
		}

		#ifdef DEBUG_DEC_FLAC_FRAME_RESIDUAL_RICE
			iprintf("rice param=%u\n", residual.partition.parameter);
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		#endif

		// see how much residuals there are
		if (residual.riceOrder == 0)
		{
			residual.partition.left = _blocksize - order;
		}
		else if (residual.riceRead == 0)
		{
			residual.partition.left = (_blocksize / (1 << residual.riceOrder)) - order;
		}
		else
		{
			residual.partition.left = _blocksize / (1 << residual.riceOrder);
		}

		if (residual.partition.encoded)
		{
			// rice-coded residuals
			if (!reader->read_rice_signed_block(&samples[order+residual.riceRead], residual.partition.left, residual.partition.parameter)) return false;
		}
		else
		{
			// unencoded residuals
			for (uint16_t i=0; i<residual.partition.left; ++i)
			{
				if (!reader->read_int32_be(&tmpS, residual.partition.parameter)) return false;
				samples[order+residual.riceRead+i] = tmpS;
			}
		}
		#ifdef DEBUG_DEC_FLAC_FRAME_RESIDUAL_RICE
			for (uint16_t i=order+residual.riceRead; i<order+residual.riceRead+residual.partition.left; i++)
			{
				iprintf("residual[%04d]=%03d\n", i-order, samples[i]);
				scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			}
		#endif
		residual.riceRead += residual.partition.left;
		residual.ricePartitionsLeft--;

//		#ifdef DEBUG_DEC_FLAC_FRAME_RESIDUAL
//			iprintf("parameter=%d (%s) %u residuals\n", residual.partition.parameter, (encoded ? "encoded" : "non-encoded"), residual.partition.left);
//			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
//		#endif
//
//		#ifdef DEBUG_DEC_FLAC_FRAME_RESIDUAL_VALUE
//			for (uint32_t z=0; z<residuals; ++z)
//			{
//				iprintf("residual[%04u]=%d\n", (residual.riceRead + z), samples[order + residual.riceRead + z]);
//				scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN) ) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
//			}
//		#endif
	}

	return true;
}

// -----------------------------
void FLACFrame::Frame::Subframe::restoreSignal(const uint16_t _blocksize)
// -----------------------------
{
	u16 i = 0;
	if (type == FLAC_SUBFRAME_CONSTANT)
	{
		#ifdef ARM9
			dmaFillWords(samples[0], &(samples[1]), ((_blocksize-1)<<2));
		#else
		for (i=1; i<_blocksize; ++i)
		{
			samples[i] = samples[0];

			#ifdef DEBUG_DEC_FLAC_FRAME_SUB_READSAMPLES
			iprintf("[flac] c[%04u]=%d\n", i, samples[i]);
			scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN) ) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
		}
		#endif
	}
	else if (type == FLAC_SUBFRAME_FIXED)
	{
		// calculate the sample
		switch (order)
		{
			case 1:
			{
				for (i=1; i<_blocksize; ++i)
				{
					// 1st order predictor (= s[t-1])
					samples[i] += samples[i-1];

					#ifdef DEBUG_DEC_FLAC_FRAME_SUB_READSAMPLES
						iprintf("[flac] f[%04u]=%d\n", i, samples[i]);
						scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN) ) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
				}
			}
			break;
			case 2:
			{
				for (i=2; i<_blocksize; ++i)
				{
					// 2nd order predictor (= 2s[t-1] - s[t-2])
					samples[i] += (samples[i-1] << 1) - samples[i-2];

					#ifdef DEBUG_DEC_FLAC_FRAME_SUB_READSAMPLES
					iprintf("[flac] f[%04u]=%d", i, samples[i]);
					scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN) ) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
				}
			}
			break;
			case 3:
			{
				for (i=3; i<_blocksize; ++i)
				{
					// 3rd order predictor (= 3s[t-1] - 3s[t-2] + s[t-3])
					samples[i] += (((samples[i-1] - samples[i-2]) << 1) + (samples[i-1] - samples[i-2])) + samples[i-3];
	
					#ifdef DEBUG_DEC_FLAC_FRAME_SUB_READSAMPLES
					iprintf("[flac] f[%04u]=%d", i, samples[i]);
					scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN) ) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
				}
			}
			break;
			case 4:
			{
				for (i=4; i<_blocksize; ++i)
				{
					// 4th order predictor (= 4s[t-1] - 6s[t-2] + 4s[t-3] - s[t-4])
					samples[i] += ((samples[i-1] + samples[i-3]) << 2) - ((samples[i-2] << 2) + (samples[i-2] << 1)) - samples[i-4];

					#ifdef DEBUG_DEC_FLAC_FRAME_SUB_READSAMPLES
					iprintf("[flac] f[%04u]=%d\n", i, samples[i]);
					scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN) ) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
					#endif
				}
			}
			break;
		}
	}
	else if (type == FLAC_SUBFRAME_LPC)
	{
		// toggle though LPCs and calculate
		s32 sum;
		u8 j = 0;

		const s32 *history;
		s32 *data = &(samples[order]);

		for (i=order; i<_blocksize; ++i)
		{
			sum = 0;
			history = data;
			for (j=0; j<order; ++j)
			{
				sum += lpc.vals[j] * (*(--history));
			}
			*(data++) += (sum >> lpc.shift);
		}
		
		#ifdef DEBUG_DEC_FLAC_FRAME_SUB_READSAMPLES
			for (i=order; i<_blocksize; ++i)
			{
				iprintf("[flac] l[%04u]=%d\n", i, samples[i]);
				scanKeys(); while (!(keysDown() & KEY_Y) && !(keysHeld() & KEY_DOWN) ) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			}
		#endif
	}
}

// -----------------------------
// 0 - no errors
// 1 - needs recalling
// 2 - stream error
int FLACFrame::Frame::Subframe::playerCall(const uint8_t _samplesize, const uint16_t _blocksize)
// -----------------------------
{
	if (!isHeaderRead)
	{
		// read the subframe's header
		if (!readHeader()) return 2;
		#ifdef DEBUG_FLAC_PROCESS
			iprintf("header ok\n");
			scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
		#endif

		switch (type)
		{
			// for constant and fixed subframes we read the
			// subframes all at once because it's raw data

			case FLAC_SUBFRAME_CONSTANT:
				if (!readConstant(_samplesize-wasted)) return 2;
				#ifdef DEBUG_FLAC_PROCESS
					iprintf("constant ok\n");
					scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
				#endif
				samplesRead = _blocksize;
			break;

			case FLAC_SUBFRAME_VERBATIM:
				if (!readVerbatim(_samplesize-wasted, _blocksize)) return 2;
				#ifdef DEBUG_FLAC_PROCESS
					iprintf("verbatim ok\n");
					scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
				#endif
				samplesRead = _blocksize;
			break;

			// for fixed and lpc subframes we need to read the data
			// in parts because we won't make it in time so first we
			// read the subframe and rice headers

			case FLAC_SUBFRAME_FIXED:
				if (!readFixed(_samplesize-wasted)) return 2;
				#ifdef DEBUG_FLAC_PROCESS
					iprintf("fixed init ok\n");
					scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
				#endif
				samplesRead = order;

				if (!readResidualInit()) return 2;
				#ifdef DEBUG_FLAC_PROCESS
					iprintf("residual partitions=%d\n", residual.ricePartitionsLeft);
					scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
				#endif
			break;

			case FLAC_SUBFRAME_LPC:
				if (!readLpc(_samplesize-wasted)) return 2;
				#ifdef DEBUG_FLAC_PROCESS
					iprintf("lpc init ok\n");
					scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
				#endif
				samplesRead = order;

				if (!readResidualInit()) return 2;
				#ifdef DEBUG_FLAC_PROCESS
					iprintf("residual partitions=%d\n", residual.ricePartitionsLeft);
					scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
				#endif
			break;
		}
		isHeaderRead = true;
	}

	if (isHeaderRead && samplesRead < _blocksize)
	{
		// we have not read all samples which means
		// we have residuals to read

		if (_blocksize > 2048)
		{
			#ifdef DEBUG_FLAC_PROCESS
				iprintf("block > 2048\n");
				scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
			// block size is <= 4096 samples
			// we split the reading into two phases
			// first we read the residuals then on the
			// second call the signal is restored

			if (!readResidualRice(_blocksize)) return 2;
			samplesRead = _blocksize;
			return 1;
		}
		else if (_blocksize > 1024)
		{
			#ifdef DEBUG_FLAC_PROCESS
				iprintf("block > 1024\n");
				scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
			// block size is <= 2048 samples
			// let's read everything in one go
			if (!readResidualRice(_blocksize)) return 2;
			samplesRead = order + residual.riceRead;
		}
		else
		{
			#ifdef DEBUG_FLAC_PROCESS
				iprintf("block <= 1024\n");
				scanKeys(); while (!(keysDown() & KEY_Y)) {scanKeys(); swiWaitForVBlank();}	swiWaitForVBlank();
			#endif
			// block size is <= 1024 samples
			// we can read everything in one go
			if (!readResidualRice(_blocksize)) return 2;
			samplesRead = order + residual.riceRead;
		}
	}

	if (isHeaderRead && samplesRead == _blocksize)
	{
		// everything read ^_^
		// we can now restore the signal
		restoreSignal(_blocksize);

		// and reset flags for another frame
		// since we won't be getting any calls
		// for this subframe
		isHeaderRead = false;
		samplesRead = 0;
		return 0;
	}
	// something still needs to be done
	else return 1;
}

// ***************************************************
// ***************************************************
//
//                      FOOTER
//
// ***************************************************
// ***************************************************


// ----------------------------------------------------------------------
bool FLACFrame::Frame::Footer::read()
// ----------------------------------------------------------------------
{
	return reader->skip_byte_block(2, true);
}

